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Abstract: In-motion alignment of Strapdown Inertial Navigation Systems (SINS) without 
any geodetic-frame observations is one of the toughest challenges for Autonomous 
Underwater Vehicles (AUV). This paper presents a novel scheme for Doppler Velocity 
Log (DVL) aided SINS alignment using Unscented Kalman Filter (UKF) which allows 
large initial misalignments. With the proposed mechanism, a nonlinear SINS error model is 
presented and the measurement model is derived under the assumption that large 
misalignments may exist. Since a priori knowledge of the measurement noise covariance is 
of great importance to robustness of the UKF, the covariance-matching methods widely 
used in the Adaptive KF (AKF) are extended for use in Adaptive UKF (AUKF). 
Experimental results show that the proposed DVL-aided alignment model is effective with 
any initial heading errors. The performances of the adaptive filtering methods are evaluated 
with regards to their parameter estimation stability. Furthermore, it is clearly shown that 
the measurement noise covariance can be estimated reliably by the adaptive UKF methods 
and hence improve the performance of the alignment. 
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1. Introduction 

With the development of high-frequency, multi-beam Doppler sonar, which can provide bottom 
velocity measurements with a precision of 0.3% or less with a update rate of up to 5Hz, a wide variety 
of Doppler-based navigation techniques have been developed [1]. A typical navigation method for 
AUVs with minimal sonar use is based on a high quality SINS combined with occasional use of a 
Doppler Velocity Log [2]. This is an open-loop system so that the initial alignment is of great 
importance to subsequent navigation operation. In the case of an AUV, the initial alignment is more 
difficult. That is because the external aiding sensors such as GPS which provide geodetic-frame 
observations are unavailable for most of the time [3]. Therefore, it is an essential task to achieve an 
accurate alignment using DVL aiding within a very short period of time. 

Due to the random wave motions as well as the dynamics of the vehicle, it is difficult to estimate 
the attitude to the accuracy of within a few degrees in a short period with the existing coarse alignment 
methods [4]. Therefore, in-motion SINS alignment with large initial misalignments is always a 
challenge and thus needs to be considered. The difficulty for the DVL-aided alignment is that DVL 
provides the velocity measurements in the Doppler instrumental frame and hence could not be used as 
the measurement for alignment directly. There are mainly two alignment schemes to solve this 
problem for small misalignments [5,6]. The first method is to establish the INS error dynamics in the 
body frame, so the velocity of the Doppler can be used as the measurements directly [5]. However, the 
INS error equations will include the unavoidable attitude error. Therefore, whether this method could 
be used for large misalignments problem needs to be analyzed. The other method is to establish the 
INS error dynamics in the navigation frame [6] and this scheme is shown in Figure 1 . The velocity of 
Doppler is transformed to the navigation frame by the attitude matrix obtained from INS. Then it can 
be used as the measurements for the Kalman filter. The problem here is that the attitude errors are 
usually very large before the alignment is finished. It will cause a large measurement error that may 
lead to the divergence of the Kalman filter [5]. But the authors in [5] failed to notice that the 
measurement error could be compensated in the measurement model. By employing this scheme, a 
new alignment model which allows large misalignments is proposed in this study. From experimental 
results, the proposed alignment model is shown to be effective with any initial attitude. 

Figure 1. DVL-aided IMU alignment scheme. 

► DVL 
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The other main effort to deal with the large misalignments problem is based on such nonlinear 
filtering methods as the so-called extended Kalman filter (EKF), unscented Kalman filter (UKF), and 
particle filter (PF). Among these nonlinear filtering methods, the UKF is wildly used due to its 
elimination of the cumbersome derivation and low computational complexity [7]. The UKF is based on 
the unscented transformation (UT) which is founded on the intuition that an approximation of a 
probability distribution is easier than that of an arbitrary nonlinear function [8]. It is able to capture the 
true mean and covariance of the Gaussian Random Variable to the 3rd order accuracy [9] and hence 
proved to be superior to EKF [10]. The most obvious shortcoming of EKF is that it requires the 
computation of Jocobian matrices and linear approximations of the EKF can be very inaccurate in 
some scenarios, leading to filter instability [9,11]. Similar to the KF [12-14] and EKF, covariance 
parameters also play an important role in the filtering performance. Therefore, several adaptive 
filtering techniques are developed to improve the performance of the UKF [15]. The covariance- 
matching method which is based on the concept of making the elements of the actual innovation 
covariance matrix consistent with their theoretical values has been shown to be one of the most 
promising techniques for KF [16]. This method is extended to UKF for its application in nonlinear 
systems [17]. However, it could not be guaranteed that the resulting measurement noise covariance is 
positive definite. Inspired by the work in [18], another covariance-matching adaptive filtering method 
based on the filter residual sequence is presented. This study first evaluates the impacts of adaptive 
filtering methods on the parameter estimation stability with different window sizes and different initial 
measurement noise covariance matrix. Experimental results demonstrate that the measurement noise 
covariance can be estimated reliably by the adaptive filtering methods. The performance of the 
adaptive filtering methods are also compared and analyzed. 

The rest of this paper is organized as follows: Section 2 is devoted to the presentation of the 
nonlinear DVL-aided IMU alignment model which can tolerate large misalignments. Section 3 
presents the mathematical formulas of the UKF and adaptive UKF techniques. In Section 4, the 
performance of the proposed algorithms are evaluated and compared with real experimental data. The 
conclusions are drawn in Section 5. 

2. Alignment Model 

2.1. INS Error Dynamics Model 

The nonlinear SINS error model proposed in [19,20] will be used in this paper. The nominal error 
state includes the velocity error ( 8v c ), the attitude error ( y/), the accelerometer bias ( V* ) and the gyro 
bias (£ b ). The formulas of this model are given as follows: 



Sv c =(I-C c p )C p b f b - (2Q< + a c ec )Sv c - M ec v c + C c b V b + Sg c 



(1) 



y/ = (I-C c p )col-C b p e b 



(2) 



(3) 



£ b =0 



(4) 
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where the superscript c denotes the local level frame at the computed position and its orientation is 
north-up-east (NUE). p , / and b are the platform frame, the inertial frame and the body frame 
respectively. C is the direction cosine matrix. f b is the accelerometer measurement. Sg c is the 
gravity error caused by the position error and it can be ignored during the process of the alignment. 
£l c ie is the skew matrix of co c ie which is the angle rate of the Earth frame relative to the inertial frame. 
of ie is given by: 

co ie cos / 
co), = co ie sin I 
0 



(5) 



where: / is the geographic latitude; co ie is the earth's rotation angular velocity. Q. c ec is the skew matrix 
of of ec which is the angle rate of the navigation frame relative to the Earth frame. of ec is given by: 



0)1 = 



V E tan / 
R 



(6) 



where R n and R m are the transverse and meridian radius of the curvature respectively; the subscripts E 
and TV denote the east and north components in the c frame. of ic is the angle rate of the navigation 
frame relative to the inertial frame. It can be obtained by: 



Define: 



of 

It 










"sin(^)" 






cos(^) 


S y 




sin(^) 


C y 




cos(^) 






sin(^ z ) 


_ C z. 




_cos(^ z )_ 



(7) 



(8) 



C c is given by: 



C - 

p 



S * S y- 



c c 

x y 



~ S x S z S y 



(9) 



In the error model presented above, all the three misalignment angles are assumed to be large. For 
real time applications, it is often the case that there are a large uncertainty in heading angle and low 
uncertainties in leveling angles [20]. 
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2.2. Measurement Model 

The velocity of Doppler in the local level frame v d can be described as follows: 



v c d =C c b C b d v d (10) 



where C d is a constant matrix which translates the velocity of Doppler v d from the Doppler instrumental 
frame to the vehicle body frame. It needs to be calibrated before a mission is conducted [21]. Supposing 
the error of v c d is mainly caused by the platform misalignments, the DVL measurement in error v d can 
be described as follows: 



v c d =ClC b d v d +SC b C b d v d 



= v d + dC b C d v d 

where the perturbation of the attitude matrix SC b is given by [22]: 

sa b ={i-c e )ci (12) 

Inserting Equation (12) into Equation (1 1), it yields: 

v d =v d + (I-C?)C c b C b v d (13) 

Differentiating the velocity of INS and DVL, the measurement model is given below: 

v c ms -v c d =v c INS +Sv c INS -v c d -{I-d)ClC b d v d 
= 8v] m -{I-Cl)ClC b d v d 

3. UKF Techniques 

3. 1. UKF in Additive Noise Case 

The considered nonlinear discrete-time system with additive noise is presented as follows [23]: 

x k=f( x k-i) + w k-i (15) 

z k=Kx k ) + v k (16) 

where x k eR n is the state vector; z k eR m is the measurement vector; /(•) and /*(•) are nonlinear 
functions; w h and v k are the uncorrected zero-mean Gaussian white sequences and their conversances are: 

E[w k w]] = d v Q k 

E[v k v]] = S^R k (17) 
E[w k v]] = Q 

With a view of reducing the computational burden, the non-augmented UKF is widely used in such 
additive noise cases [7,24-26]. The resemblance between the UKF and the KF is that the 
implementations of the two algorithms all consist of the prediction of the state mean and covariance, 
and then the update with the measurements [27]. The implementation of the non- augmented UKF 
algorithm is given as follows [25,26]: 



Sensors 2013, 13 



1051 



(a) Initialization: 



(b) Time-updating: 



x 0 — E[x 0 ], P 0 — E^(x 0 x 0 )(x 0 x 0 ) J 



Xk-\ ~ 



Xi,k\k-\ f(Xi,k-\ ) 
r = Y 2L W (>n)y* 

^k\k-l ~ S/=o ^ } {Xi,k\k-\ ~ *k\k-l ) {Zi,k\k-\ ~ *k\k-l ) + Qk-\ 
Xk\k-\ ~ \^k\k-\ \j c k\k-\ ~\ L + Tyj Pk\k-i \_^k\k-\ ~\ L ~ TyJ ^k\k-l J 
^li,k\k-\ = h(Zi,k\k-l) 

z =y 2L w (m) n 

^k\k-\ Lui=§ i >i,k\k-\ 



(c) Measurement-updating: 

^kh ~ S/=0 ^ ^ {^i,k\k-l ~ ^k\k-\ ) {jli,k\k-\ ~ ^k\k-\ ) + ^ 
= S*=0 ^ } {Xi,k\k-\ ~ *k\k-l ) {jli,k\k-\ ~ ^k\k-\ ) 

K x k z k z k z k 

*k = *k\k-l + ^k{ Z k~ Z k\k-\ ) 
= ^k\k-l ~ K k P iti K k 

The parameters for calculating the sigma-points are given as follows: 

X = a 2 [L + k)-L 

W 0 {m) =XI{L + X) 
W^ c) =AI{L + X) + (\-cc 2 +p) 
W (m) =W ic) = i/[2(£ + A)], (/ = 1,2,...,2£) 



(18) 

(19) 
(20) 
(21) 
(22) 
(23) 
(24) 
(25) 

(26) 
(27) 
(28) 
(29) 
(30) 



(31) 



where W {m) and ^ (c) represent the mean weight and covariance weight, respectively [15]; L is the 
dimension of the state vector; is the i th column of the matrix square root of P k]k _ x ; a 

determines the spread of the sigma points around x and is usually set to a small positive value (e.g., 
le-3); k is a secondary scaling parameter which is usually set to 0; j3 is used to incorporate prior 
knowledge of the distribution of x (for Gaussian distributions, /? = 2 is optimal) [28]. 



Sensors 2013, 13 



1052 



3.2. Innovation-Based Adaptive UKF 

The innovation sequence d k which is the difference between the measurement z k and its predicted 
value z k \ h _ x is given as follows: 

d k = z k-^k\ k -x (32) 
where z klk _ x is obtained from Equation (25). By matching the covariance matrix of the innovation 
sequence with its theoretical form, P. . can be calculated as follows [17]: 

1 k 

^=77 ^ d j d j (33) 

iV j=k-N+\ 

Nis the length of the sliding sampling window. By replacing the Equation (26) with the Equation (33), 
the Adaptive UKF (AUKF) algorithm is obtained. This technique is similar to the traditional 
innovation-based adaptive Kalman filter (AKF) [16,29]. By comparing the Equation (33) with (26), the 
estimation of the measurement noise covariance can be obtained as follows: 

R k = 77 Z d j d j ~ HZ W i C) (Viw-i ~ *k\ k -i ) (Vmk-i ~ *k\k-i f (34) 

iV j=k-N+\ 

As can be seen from Equation (34), the estimated measurement noise covariance is the difference 
between two positive definite matrices, it cannot be guaranteed that the resulting matrix R k is positive 
definite. This trouble can easily cause the failure of the filter in real time applications. 

3.3. Residual-Based Adaptive UKF 

The residual sequence could also be used with the purpose of obtaining a realistic estimator of the 
measurement noise covariance. The residual sequence £ k is defined as follows: 

£k= z k- s k (35) 
where z k can be obtained by the estimated values x k (not the predicted values x k ^_ x ) by an extra UT: 

(36) 

7 li,k=KX U k) (37) 

^=s;:^a 08) 

by extending concept of the residual-based AKF [18] to UKF, a recursive estimator of R k can be 
obtained as: 

R k =^ j J, N j/j +Z«^ (c) (^- f 0(^- 5 *) r (39) 

It can be used in the computation of epoch k + 1. Compared to Equation (34), as Equation (39) is 
the sum of two positive definite matrices, the estimated covariance matrix is always positive definite. 
A slight disadvantage of this method is that it requires some extra computation for z k which is not 
generated by the standard UKF process. The residual-based method utilizes the measurement of the 
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previous N epoch, whereas the innovation-based method utilizes current and the previous N-l epoch. 
Therefore, the innovation sequence and the residual sequence have to be ergodic and stationary over 
the N steps. Otherwise, the performance of UKF will be degraded. 

4. Experimental Results and Discussions 

4.1. Test Configuration 

The ship-mounted experimental data were collected to evaluate the performance of the in-motion 
alignment. The experiment was carried out in Yangzi River. The equipped sensors are listed as follows: 

(a) IMU: Consists of three ring laser gyroscopes with drift rate 0.01° / h(la) and three quartz 
accelerometers with bias 5xl0~ 5 g(lcr) . Its update rate is 200 Hz. 

(b) Bottom-lock Doppler: Provides three-axis transformation velocities with accuracy ±5%o of speed 
and update rates up to 1 Hz. 

(c) GPS receiver: Provides velocity with precision of about O.lm/s, position with precision of about 
10 m, and update rates up to 1 Hz. 

In the experiment, the IMU and the GPS receiver were set up on a vessel. The DVL module was put 
beneath lm underwater. The fixing and level arm parameters of devices are shown in Figure 2. The 
flowchart of the alignment procedure is shown in Figure 3. As the update rate of the INS is much 
higher than DVL, the measurement-updating is executed only when the DVL is available. By using the 
close-loop filtering scheme, the filtering corrections are fed back every measurement-updating. If the 
filtering epoch is smaller than the sampling window size of innovation or residual, AUKF is working 
at the mode of UKF. 



Figure 2. Fixing of experimental devices. 
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Figure 3. Alignment flowchart. 
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4.2. Alignment Results by UKF 

Figure 4 shows the error curves of heading with initial heading errors from 10 degrees to 
180 degrees. As can be seen from the figure, all the error curves converged with time. The proposed 
alignment model is shown to be effective with any initial attitude. 

Figure 4. Heading error comparison with different initial heading errors. 
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Figure 5 shows convergence time for different initial heading errors with a converged heading error 
of less than 0.1 degree. It is clearly shown that larger initial heading error needs more time to 
converge. In addition, if the initial heading error is larger than 110 degrees. It is hard to guarantee that 
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the final converged heading error would reach 0.1 degree within the 900 s alignment. Figure 6 shows 
the heading error comparison with extra large initial heading errors. The alignment time was extended 
to 3,000 s. Though all the estimates converged with time, the heading errors were 0.23 degree, 0.72 
degree and 0.73 degree with initial heading error of 160 degrees, 170 degrees and 180 degrees 
respectively. It is clearly shown that the heading errors converged faster at the beginning and then the 
speed of the convergence slowed down. This was caused by the fading of the Kalman Filter gain [30]. 
Therefore, further study is still needed to improve the performance of the UKF. Moreover, providing a 
relatively accurate initial attitude will be beneficial to the alignment. 

Figure 5. Convergence time comparison with different initial heading errors. 
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Figure 6. Heading error comparison with extra large initial heading errors. 
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4.3. Measurement Noise Covariance Estimation 



A test is done by intentionally adding large initial attitude errors (100 degrees for heading, 1 degree 
for roll and pitch). Figures 7 and 8 show the estimations of measurement noise covariance with different 
window sizes by the innovation-based and the residual-based AUKF respectively. The trends of the 
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estimations obtained by these two methods are similar. As can be seen from the figures, the estimations 
of the measurement noise are unstable at the beginning and then converge with time. Finally, all the 
estimates converge to the value of around 0.01 (m 2 /s 2 ). It can also be seen from the figures that the 
estimation oscillation becomes obvious when shorter window sizes are used. It illustrates that a short 
window size may lead to unstable estimation. This is similar to the conclusions of AKF [16]. 



Figure 7. Estimation of measurement noise covariance with different window sizes by 
innovation-based AUKF. 
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Figure 8. Estimation of measurement noise covariance with different window sizes by 
residual-based AUKF. 
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Figure 9. Estimation of measurement noise covariance with different initial R values by 
innovation-based AUKF. 
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Figure 10. Estimation of measurement noise covariance with different initial 7? values by 
residual-based AUKF. 
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A test is done by intentionally adding large initial attitude errors (100 degrees for heading, 1 degree 
for roll and pitch). Figures 7 and 8 show the estimations of measurement noise covariance with 
different window sizes by the innovation-based and the residual-based AUKF respectively. The trends 
of the estimations obtained by these two methods are similar. As can be seen from the figures, the 
estimations of the measurement noise are unstable at the beginning and then converge with time. 
Finally, all the estimates converge to the value of around 0.01 (m 2 /s 2 ). It can also be seen from the 
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figures that the estimation oscillation becomes obvious when shorter window sizes are used. It 
illustrates that a short window size may lead to unstable estimation. This is similar to the conclusions 
of AKF [16]. 

Figures 9 and 10 illustrate the estimates of the measurement noise co variance with different initial 
R by the AUKF techniques. The window size used for estimation was 100. It is clearly shown that the 
initial values of R have some impacts only during the filter convergence periods. Finally, all the 
curves converged and closely matched each other. The requirement of a priori knowledge of the 
measurement noise covariance is alleviated. In addition, it is clearly shown by Figures 7-10, the 
estimated measurement noise covariance obtained by the innovation based method almost approaches 
that obtained by the residual based case. There is no decided difference between them. 

4.4. Performance Evaluation of the Adaptive UKF Techiniques 

A test was designed to evaluate the performance of the estimated measurement noise covariance. 
The initial attitude error was 100 degrees for heading, 1 degree for roll and pitch, respectively. As can 
be seen from Figures 7 and 8, the values of the estimated measurement noise covariance were around 
0.01 (m 2 /s 2 ). Therefore, this value was applied in the alignment [R = diag(0. 01,0. 01,0. 01) (m 2 /s 2 )]. In 
addition, the alignment was also executed with a larger R [R = diag(0. 1,0. 1,0.1) (m 2 /s 2 )] and a smaller 

2 2 

R [R = diag(0.001,0.001,0.001) (m7s z )]. Figure 11 shows the error curves of the heading with different 
R. As can be seen from the figure, all the estimates converged with time. But the heading error with R 

2 2 2 2 

value of 0.01 (m /s ) converged more rapidly than that with the value of 0.1 (m /s ) and 0.001 
(m 2 /s 2 ).As shown in Table 1, it took 676 seconds for the heading error to converge within 0.1 degree 

9 9 9 9 9 9 

with R value of 0.01 (m /s ) while the convergence time for R value of 0.1 (m /s ) and 0.001 (m /s ) 
were 766 s and 800 s respectively. In a sense, the estimated measurement noise covariance is proved to 
be realistic. Furthermore, it's clearly shown that the measurement noise covariance plays an important 
part in the performance of the UKF. Once the R is correctly estimated, it can improve the performance 
of the alignment. 

Figure 11. Heading error comparisons with different R. 
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Table 1. Performance comparisons with different/? values. 
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Figure 12. Attitude error comparison between UKF and AUKFs with initial attitude error 
of [1°, 100°, 1°]. 
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The tests were designed to compare the performance of the UKF and AUKF techniques for their 
applications in the DVL-aided SINS alignment problem. An example is shown in Figure 12. In the test, 
the initial attitude error was 100 degrees for heading and 1 degree for leveling. The initial R value 

2 2 

applied in UKF and AUKF was 0.01 (m7s z ). The window size for both the innovation-based and 
residual-based AUKF was 100. As can be seen from Figure 12, the estimated results of the UKF and 
the AUKF were different after 100 s when the AUKF methods were switched to the adaptive 
algorithms. Due to the setting of R [0.01 (m 2 /s 2 )] given for the UKF was very close to the optimal 
values obtained from the above tests, the trends of the error curves obtained by the UKF and the 
AUKF methods are similar. Partial magnification of the heading errors are shown in Figure 13. It is 
clearly shown that the heading error estimated by the UKF closely matches that predicted by the 
AUKF methods with the increase of the time. It means that the distinctions between the 
performances of the UKF and the AUKF are small if a appropriate measurement noise covariance 
[R = diag(0. 01,0. 01,0. 01) (m 2 /s 2 )] is applied. However, in the case of underwater vehicles, it's very 
difficult to obtain a prior knowledge of the measurement noise covariance due to unstable Doppler 
measurements. Though a proper R is employed, AUKF methods still performed a little better UKF. 
Compared with 676 seconds, it took 667 and 664 seconds for the heading errors to converge within 
0.1 degree by the innovation based and the residual based AUKF, respectively. As can be seen from 
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Figure 13, their final heading errors are also smaller than UKF. In addition, there is no notable contrast 
between the AUKF methods. The error curve that predicted by innovation based AUKF is exactly 
parallel to that obtained by residual based AUKF. 

Figure 13. Partial magnification of the heading error. 
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Figure 14. Heading accuracy comparison with different window sizes. 
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Regarding the attitude obtained from a high precision INS/GPS integration as reference, Figure 14 
shows the heading accuracy comparison with different window sizes in the 900 s alignments. It is 
clearly shown that the estimated results meet the requirement of the alignment of 0.1 degree for the 
heading error. Choosing an optimal window size is of great importance. However, how to determine 
the optimal window size is still a challenge issue. It can now only be determined by simulations or 
experiences. As can be seen from Figure 14, the trends of the curves obtained by the innovation-based 
and residual-based AUKFs are similar. Accompanying with oscillations, the heading accuracy slowly 
increased until the window size was around 100. After that, the heading accuracy slowly decreased to 
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0. 03 degree with larger the window size. Then, the heading accuracy remained around 0.03 degree. 
In addition, the oscillations was obvious when the window size was smaller than 100. This was caused 
by the unstable estimation of the measurement noise covariance with small window sizes. The result 
presented in Figure 14 meets our knowledge that the small window size will lead to the biased 
estimation of the measurement noise covariance while very large window size may cause the adaptive 
filter loosing the ability of adaptation [29]. Moreover, for this specific case, a slight difference between 
the innovation-based and residual-based AUKF methods is that the window size of 95 performed best 
for innovation-based AUKF while window size of 100 performed best for the residual-based AUKF. 

5. Conclusions 

This paper has presented a new alignment scheme for the DVL-aided SINS in-motion alignment 
which allows large initial misalignments. From the experimental data, it has been clearly shown that 
the proposed alignment model can be applied for the DVL-aided SINS in-motion alignment with any 
initial heading errors. As the measurement noise covariance is of great importance to the performance 
of the UKF, the covariance-matching methods applied in AKF have been extended for use in the 
Adaptive UKF. By using innovation-based and residual-based AUKF techniques, the measurement 
noise covariance can be estimated reliably and hence can improve the performance of the UKF. Its 
performance has been demonstrated with field experimental data. 
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